#!/usr/bin/env python
# -*- coding: utf-8 -*
from control.msg import gpsr_msg
import rospy
import roslib
import os
import sys


class Conrtol:
    def __init__(self):
        msg = gpsr_msg()
        self.nor_state = msg.Receiving
        self.next_state = msg.GoToLoc1
        self.need_hep = False
        self.control_sub = rospy.Subscriber("/control_center", gpsr_msg, self.controlCallback)
        self.control_pub = rospy.Publisher("/control_center", gpsr_msg,queue_size=1)
		# 开启语音识别模块
        os.system("gnome-terminal -x bash -c 'roslaunch speech speech_control.launch'")
        # 开启yolo图像识别模块
        os.system("gnome-terminal -x bash -c 'roslaunch darknet_ros darknet_ros.launch'")
        # 开启导航模块
        # 使用turtlebot
        # os.system("gnome-terminal -x bash -c 'roslaunch control navigation.launch'")
        # 使用先锋机器人
        os.system("gnome-terminal -x bash -c 'roslaunch move_base_benchmark move_base_benchmark.launch'")
        # 开启导航任务控制模块
        os.system("gnome-terminal -x bash -c 'rosrun control navigation.py'")
        
        msg.NowTask = msg.Receiving
        msg.NextTask = msg.GoToLoc1
        msg.NeedHelp = False
        msg.FinishState = False
		
        print("=======Start Task Reception=======")
        rospy.sleep(10.0)
        self.control_pub.publish(msg)
        print("-------Receiving-------")

    def controlCallback(self, msg):
        if msg.FinishState == True:
            n_msg = gpsr_msg()
            n_msg.NeedHelp = False
            n_msg.FinishState = False

            # 1 GoToLoc1
            if(msg.NowTask == n_msg.Receiving and self.nor_state == n_msg.Receiving):
                n_msg.NowTask = n_msg.GoToLoc1
                n_msg.NextTask = n_msg.Speaking1
                print("-------GoToLoc1-------")
                self.nor_state = n_msg.NowTask
                self.next_state= n_msg.NextTask
                self.control_pub.publish(n_msg)

            # 2 Speaking1
            elif msg.NowTask == n_msg.GoToLoc1 and self.nor_state == n_msg.GoToLoc1:
                # 关闭导航节点
                # 打开语音节点——发布control
                n_msg.NowTask = n_msg.Speaking1
                n_msg.NextTask = n_msg.GoToLoc2
                print("-------Speaking1-------")
                self.nor_state = n_msg.NowTask
                self.next_state= n_msg.NextTask
                self.control_pub.publish(n_msg)

            # 3 GoToLoc2
            elif msg.NowTask == n_msg.Speaking1 and self.nor_state == n_msg.Speaking1:
                n_msg.NowTask = n_msg.GoToLoc2
                n_msg.NextTask = n_msg.Speaking2
                print("-------GoToLoc2-------")
                self.nor_state = n_msg.NowTask
                self.next_state= n_msg.NextTask
                self.control_pub.publish(n_msg)

            # 4 Speaking2
            elif msg.NowTask == n_msg.GoToLoc2 and self.nor_state == n_msg.GoToLoc2:			
                n_msg.NowTask = n_msg.Speaking2
                n_msg.NextTask = n_msg.Receiving
                print("-------Speaking2-------")
                self.nor_state = n_msg.NowTask
                self.next_state= n_msg.NextTask
                self.control_pub.publish(n_msg)
          
            # 5 Finish
            elif msg.NowTask == n_msg.Speaking2 and self.nor_state == n_msg.Speaking2:                
                print("=======Finish Task=======")
            


if __name__ == '__main__':
    rospy.init_node('control_task', anonymous=True)
    control_ = Conrtol()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")

